{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "Failed to load Python extension for LZ4 support. LZ4 compression will not be available.\n"
     ]
    },
    {
     "ename": "FileNotFoundError",
     "evalue": "[Errno 2] No such file or directory: '/home/xuhao/Dropbox/data/TRO2020-SwarmLocal/tro2020-bags/pre/drone1-swarm_local_2020-12-13-21-31-51.bag'",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mFileNotFoundError\u001b[0m                         Traceback (most recent call last)",
      "\u001b[0;32m<ipython-input-1-578e54d03849>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m     13\u001b[0m \u001b[0;31m#Moveout node_detected\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     14\u001b[0m \u001b[0;32mwith\u001b[0m \u001b[0mrosbag\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mBag\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0moutput_bag\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m'w'\u001b[0m\u001b[0;34m)\u001b[0m \u001b[0;32mas\u001b[0m \u001b[0moutbag\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 15\u001b[0;31m     \u001b[0;32mfor\u001b[0m \u001b[0mtopic\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mmsg\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mt\u001b[0m \u001b[0;32min\u001b[0m \u001b[0mrosbag\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mBag\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0minput_bag\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mread_messages\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m     16\u001b[0m         \u001b[0;32mif\u001b[0m \u001b[0mtopic\u001b[0m \u001b[0;32min\u001b[0m \u001b[0mmoveout_topics\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     17\u001b[0m             \u001b[0;32mpass\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m/opt/ros/melodic/lib/python2.7/dist-packages/rosbag/bag.py\u001b[0m in \u001b[0;36m__init__\u001b[0;34m(self, f, mode, compression, chunk_threshold, allow_unindexed, options, skip_index)\u001b[0m\n\u001b[1;32m    465\u001b[0m         \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0m_encryptor\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0m_ROSBagNoEncryptor\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    466\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 467\u001b[0;31m         \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0m_open\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mf\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mmode\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mallow_unindexed\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m    468\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    469\u001b[0m         \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0m_output_file\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0m_file\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m/opt/ros/melodic/lib/python2.7/dist-packages/rosbag/bag.py\u001b[0m in \u001b[0;36m_open\u001b[0;34m(self, f, mode, allow_unindexed)\u001b[0m\n\u001b[1;32m   1425\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m   1426\u001b[0m         \u001b[0;32mtry\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m-> 1427\u001b[0;31m             \u001b[0;32mif\u001b[0m   \u001b[0mmode\u001b[0m \u001b[0;34m==\u001b[0m \u001b[0;34m'r'\u001b[0m\u001b[0;34m:\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0m_open_read\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mf\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mallow_unindexed\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m   1428\u001b[0m             \u001b[0;32melif\u001b[0m \u001b[0mmode\u001b[0m \u001b[0;34m==\u001b[0m \u001b[0;34m'w'\u001b[0m\u001b[0;34m:\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0m_open_write\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mf\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m   1429\u001b[0m             \u001b[0;32melif\u001b[0m \u001b[0mmode\u001b[0m \u001b[0;34m==\u001b[0m \u001b[0;34m'a'\u001b[0m\u001b[0;34m:\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0m_open_append\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mf\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mallow_unindexed\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m/opt/ros/melodic/lib/python2.7/dist-packages/rosbag/bag.py\u001b[0m in \u001b[0;36m_open_read\u001b[0;34m(self, f, allow_unindexed)\u001b[0m\n\u001b[1;32m   1447\u001b[0m             \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0m_filename\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0;32mNone\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m   1448\u001b[0m         \u001b[0;32melse\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m-> 1449\u001b[0;31m             \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0m_file\u001b[0m     \u001b[0;34m=\u001b[0m \u001b[0mopen\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mf\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m'rb'\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m   1450\u001b[0m             \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0m_filename\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mf\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m   1451\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mFileNotFoundError\u001b[0m: [Errno 2] No such file or directory: '/home/xuhao/Dropbox/data/TRO2020-SwarmLocal/tro2020-bags/pre/drone1-swarm_local_2020-12-13-21-31-51.bag'"
     ]
    }
   ],
   "source": [
    "import rosbag\n",
    "PATH=\"/home/xuhao/Dropbox/data/TRO2020-SwarmLocal/tro2020-bags/pre/\"\n",
    "PATHOUT=\"/home/xuhao/bags/swarm_local_2020_12/\"\n",
    "input_bag = PATH +\"drone1-swarm_local_2020-12-13-21-31-51.bag\"\n",
    "output_bag = PATHOUT + \"drone1-swarm_local_2020-12-13-21-31-51_processed.bag\"\n",
    "moveout_topics = {\n",
    "    \"/swarm_drones/node_detected\",\n",
    "    \"/swarm_drones/swarm_drone_fused\",\n",
    "    \"/swarm_drones/swarm_drone_fused_relative\",\n",
    "    \"/swarm_drones/swarm_frame\",\n",
    "    \"/swarm_drones/swarm_frame_predict\"\n",
    "}\n",
    "#Moveout node_detected\n",
    "with rosbag.Bag(output_bag, 'w') as outbag:\n",
    "    for topic, msg, t in rosbag.Bag(input_bag).read_messages():\n",
    "        if topic in moveout_topics:\n",
    "            pass\n",
    "        else:\n",
    "            #outbag.write(topic, msg, msg.header.stamp if msg._has_header else t)\n",
    "            outbag.write(topic, msg, t)            "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Bag drone1.bag start at 1614678030.9677083\n",
      "Bag drone2.bag start at 1614678029.0100212\n",
      "Will use 1614678030.9677083 as start\n",
      "Dt between drones 0.0 bag drone1.bag 1614674811598963544 1614674811598963544\n",
      "Dt between drones 1.262796552 bag drone2.bag 1614674812861760096 1614674811598963544\n",
      "Converting drone1.bag\n",
      "Converting drone2.bag\n"
     ]
    }
   ],
   "source": [
    "import rosbag\n",
    "import os\n",
    "import rospy\n",
    "#PATH=\"/home/xuhao/bags/swarm_local_2020_12/\"\n",
    "#PATH=\"/home/xuhao/Dropbox/data/TRO2020-SwarmLocal/2021-3-1-outdoor/trial3/\"\n",
    "PATHOUT=\"/home/xuhao/bags/swarm_local_2021-3-2/trial3/\"\n",
    "bags = [\"drone1.bag\",\n",
    "                \"drone2.bag\",\n",
    "                \"drone2.bag\"\n",
    "       ]\n",
    "t_offset = 10\n",
    "#t_offset = 90\n",
    "\n",
    "t0 = 0\n",
    "\n",
    "for bag in bags:\n",
    "    for topic, msg, t in rosbag.Bag(PATH+bag).read_messages():\n",
    "        print(f\"Bag {bag} start at {t.to_sec()}\")\n",
    "        if t.to_sec() > t0:\n",
    "            t0 = t.to_sec()\n",
    "            break\n",
    "        else:\n",
    "            break\n",
    "print(\"Will use\", t0, \"as start\")\n",
    "\n",
    "ros_t0 = None\n",
    "lps_t1 = None\n",
    "moveout_topics = {\n",
    "    \"/uwb_node/incoming_broadcast_data\"\n",
    "}\n",
    "\n",
    "dts = {bags[0]:0}\n",
    "\n",
    "for topic, msg, t in rosbag.Bag(PATH+bags[0]).read_messages():\n",
    "    if t.to_sec() > t0 + t_offset and topic == \"/uwb_node/time_ref\":\n",
    "        _t0 = msg.header.stamp\n",
    "        _t1 = msg.time_ref\n",
    "        dt0 = _t0 - _t1\n",
    "        break\n",
    "\n",
    "for bag_name in bags:\n",
    "    for topic, msg, t in rosbag.Bag(PATH+bag_name).read_messages():\n",
    "        if  t.to_sec() > t0 + t_offset and topic == \"/uwb_node/time_ref\":\n",
    "            _t0 = msg.header.stamp\n",
    "            _t1 = msg.time_ref\n",
    "            dt1 = _t0 - _t1\n",
    "            break\n",
    "    dt = (dt1 - dt0)\n",
    "    print(f\"Dt between drones {dt.to_sec()} bag {bag_name}\", dt1, dt0)\n",
    "    dts[bag_name] = dt\n",
    "\n",
    "for bag in bags:\n",
    "    print(f\"Converting {bag}\")\n",
    "    #cmd = f\"/opt/ros/melodic/bin/rosbag filter {PATH}{bag} {PATHOUT}filtered-{bag} 't.to_sec() >= {t}'\"\n",
    "    #print(os.popen(cmd).read())\n",
    "    _dt = rospy.Time(0)\n",
    "    if bag != bags[0]:\n",
    "        _dt = dts[bag]\n",
    "        #dt = 0\n",
    "    output_bag = PATHOUT+\"filtered-\"+bag;\n",
    "    with rosbag.Bag(output_bag, 'w') as outbag:\n",
    "        for topic, msg, t in rosbag.Bag(PATH+bag).read_messages():\n",
    "            if topic in moveout_topics:\n",
    "                pass\n",
    "            else:\n",
    "                t = msg.header.stamp - _dt if msg._has_header else t - _dt\n",
    "                if msg._has_header:\n",
    "                    msg.header.stamp = msg.header.stamp - _dt\n",
    "                if t.to_sec() > t0 + t_offset:\n",
    "                    outbag.write(topic, msg, t)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "jupyter": {
     "source_hidden": true
    }
   },
   "outputs": [],
   "source": [
    "ros_t0 = None\n",
    "lps_t1 = None\n",
    "for bag in bags:\n",
    "    output_bag = PATHOUT+\"filtered-\"+bag;\n",
    "    with rosbag.Bag(output_bag, 'w') as outbag:\n",
    "        for topic, msg, t in rosbag.Bag(PATH+bag).read_messages():\n",
    "            if t < t0:\n",
    "                continue\n",
    "            if bag ==  \"drone2-swarm_local_raw_2020-12-12.bag\":\n",
    "                if topic == \"/uwb_node/remote_nodes\":\n",
    "                    if ros_t0 is None:\n",
    "                        ros_t0 = msg.header.stamp.to_sec()\n",
    "                    if lps_t1 is None:\n",
    "                        lps_t1 = msg.sys_time\n",
    "                    lps_time = (msg.header.stamp.to_sec() - ros_t0)*1000 + lps_t1\n",
    "                    #print(msg.sys_time, lps_time)\n",
    "                    msg.sys_time = int(lps_time)\n",
    "            outbag.write(topic, msg, msg.header.stamp if msg._has_header else t)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Converting drone1.bag\n",
      "Converting drone2.bag\n"
     ]
    }
   ],
   "source": [
    "for bag in bags:\n",
    "    print(f\"Converting {bag}\")\n",
    "    #cmd = f\"/opt/ros/melodic/bin/rosbag filter {PATH}{bag} {PATHOUT}filtered-{bag} 't.to_sec() >= {t}'\"\n",
    "    #print(os.popen(cmd).read())\n",
    "    _dt = rospy.Time(0)\n",
    "    if bag != bags[0]:\n",
    "        _dt = dts[bag]\n",
    "    output_bag = PATHOUT+\"filtered-\"+bag;\n",
    "    with rosbag.Bag(output_bag, 'w') as outbag:\n",
    "        for topic, msg, t in rosbag.Bag(PATH+bag).read_messages():\n",
    "            if topic in moveout_topics:\n",
    "                pass\n",
    "            else:\n",
    "                t = msg.header.stamp - _dt if msg._has_header else t - _dt\n",
    "                if msg._has_header:\n",
    "                    msg.header.stamp = msg.header.stamp - _dt\n",
    "                if t.to_sec() > t0 + t_offset:\n",
    "                    outbag.write(topic, msg, t)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
